/* USER CODE BEGIN Header */
/**
    ******************************************************************************
    * @file                     : main.c
    * @brief                    : Main program body
    ******************************************************************************
    * @attention
    *
    * <h2><center>&copy; Copyright (c) 2020 STMicroelectronics.
    * All rights reserved.</center></h2>
    *
    * This software component is licensed by ST under BSD 3-Clause license,
    * the "License"; You may not use this file except in compliance with the
    * License. You may obtain a copy of the License at:
    *                                                opensource.org/licenses/BSD-3-Clause
    *
    ******************************************************************************
    */
/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "adc.h"
#include "can.h"
#include "dma.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "simplelib.h"
#include "laser.h"
#include "chassis.h"
#include "vega.h" //test
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
static int time_1ms_cnt = 0;
Clock clock = {0};
void clock_exe()
{
  clock.sec += clock.m_sec / 100;
  clock.min += clock.sec / 60;
  clock.sec %= 60;
  clock.m_sec %= 100;
}
/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */

/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
// flag用来决定启用哪些模块，响应模块的执行函数会扫描flag中对应位置的值，�???????????????0则不执行
Flag flag = {
    0, //main_flag
    1, //chassis_control_flag
    0, //chassis_handle_flag
    0, //chassis_auto_flag
    0, //chassis_laser_flag
    0, //lcd_flag
    1, //m3508_flag
    1, //vesc_flag
    0, //clock_1s_flag
    0, //clock_10ms_flag
    0  //stop_flag
};
int go_to_point_flag = 1;
int adjust_mode = 0;
int first_to_go = 3; //1 is left , 3 is right;
float test_value[10] = {100, 30, 30, 0.8, 5, 3000, 125000, 0.75, 1};
/**
 * @brief 
 * 0 
 * 1 
 * 2 
 * 3 PM_chassis_move_trace中输出速度的倍率
 * 4 PM_chassis_move_trace判断到达的倍数
 * 5
 * 6
 * 7
 * 8
 * 
 */
/* USER CODE END 0 */

/**
    * @brief    The application entry point.
    * @retval int
    */
int main(void)
{
  /* USER CODE BEGIN 1 */

  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_DMA_Init();
  MX_USART1_UART_Init();
  MX_CAN1_Init();
  MX_UART5_Init();
  MX_ADC1_Init();
  MX_TIM3_Init();
  MX_USART3_UART_Init();
  MX_ADC3_Init();
  MX_ADC2_Init();
  /* USER CODE BEGIN 2 */
  simplelib_init(&huart1, &hcan1);
  laser_init();
  chassis_init();

  if (!HAL_GPIO_ReadPin(S1_GPIO_Port, S1_Pin))
  { //0v
    uprintf("go left first\r\n");
    first_to_go = 1;

    PM_route_status = ROUTE_START_TO_LEFT;
    // test_PM(1);
  }
  else
  {
    uprintf("go right first\r\n");
    first_to_go = 3;
    PM_route_status = ROUTE_START_TO_RIGHT;
    // test_PM(3);
  }
  flag.main_flag = 1;

  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
    simplelib_run();
    clock_exe(); // 时钟
    laser_exe();
    if (!adjust_mode)
    {
      // test_chassis_exe();
      PM_chassis_exe();
      PM_exe();
    }
    else
    {
      static int cnt = 0;
      if (flag.m3508_flag)
      {
        chassis_pos_update();
        flag.m3508_flag = 0;
        if (cnt % 3 == 0)
          ReadActualPos(CAN1, 5);
        else if(cnt % 3 == 1)
          ReadActualPos(CAN1, 6);
        else 
          ReadActualPos(CAN1, 7);
        cnt++;

      }
    }
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
    if (flag.clock_1s_flag)
    {
      if (chassis_status.vega_is_ready)
        vega_print_pos();
      else
        uprintf("Not Ready\r\n");
      laser_print_distance();
      laser_print_adc();
      flag.clock_1s_flag = 0;
    }
  }
  /* USER CODE END 3 */
}

/**
    * @brief System Clock Configuration
    * @retval None
    */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /** Configure the main internal regulator output voltage 
    */
  __HAL_RCC_PWR_CLK_ENABLE();
  __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);
  /** Initializes the CPU, AHB and APB busses clocks 
    */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLM = 4;
  RCC_OscInitStruct.PLL.PLLN = 168;
  RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
  RCC_OscInitStruct.PLL.PLLQ = 4;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }
  /** Initializes the CPU, AHB and APB busses clocks 
    */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK)
  {
    Error_Handler();
  }
}

/* USER CODE BEGIN 4 */
void inc(void)
{

  if (flag.main_flag == 1)
  {
    time_1ms_cnt++;
    PM_cnt_ms++;
    flag.chassis_laser_flag = 1;
    //1000ms
    if (time_1ms_cnt % 1000 == 0)
    {
      flag.clock_1s_flag = 1;
    }
    //500ms
    if (time_1ms_cnt % 500 == 0)
    {
      HAL_GPIO_TogglePin(LED1_GPIO_Port, LED1_Pin); //led闪烁
    }
    //100ms
    if (time_1ms_cnt % 100 == 0)
    {
    }
    if (time_1ms_cnt % 10 == 0)
    {

      flag.clock_10ms_flag = 1;
      clock.m_sec++;
    }

    //5ms
    if (time_1ms_cnt % 5 == 0)
    {

      flag.m3508_flag = 1;
      flag.lcd_flag = 1;
      flag.chassis_laser_flag = 1;

      if (chassis_status.vega_is_ready == 1)
      {
        flag.chassis_control_flag = 1;
      }
      // flag.vesc_flag = 1;

      // flag.m2006_flag = 1;
    }

    //vega（全场定位）初始化时间设定，要有15s的启动时
    if (time_1ms_cnt % 20000 == 0 && chassis_status.vega_is_ready == 0)
    {
      chassis_status.vega_is_ready = 1;
      if (adjust_mode)
      {
        MotorOff(CAN1, 5);
        uprintf("motor5 is off ");
        MotorOff(CAN1, 6);
        uprintf("motor6 is off ");
        MotorOff(CAN1, 7);
        uprintf("motor7 is off ");
      }
      else{
        PM_init();
        scan(first_to_go);
      }
        
      HAL_GPIO_TogglePin(LED3_GPIO_Port, LED3_Pin);
      uprintf("Vega init done!!!\r\n");
      

      //TODO
      // PM_start_flag = 1;
      // PM_num = 3;      
    }
    if (time_1ms_cnt >= 60000) // 防止int类型溢出
    {
      time_1ms_cnt = 0;
    }
  }
}
/* USER CODE END 4 */

/**
    * @brief    This function is executed in case of error occurrence.
    * @retval None
    */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */

  /* USER CODE END Error_Handler_Debug */
}

#ifdef USE_FULL_ASSERT
/**
    * @brief    Reports the name of the source file and the source line number
    *                 where the assert_param error has occurred.
    * @param    file: pointer to the source file name
    * @param    line: assert_param error line source number
    * @retval None
    */
void assert_failed(uint8_t *file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
         tex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */

/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
